function [ out, auxOut ] = sinistep_pgs( mode, in, cfg, par, initCon, cst ) %#codegen
%SINISTEP_PGS 捷联惯性导航积分算法解算周期
%
% Tips
% # E系GwEN，初始N系ENU，初始L系NED
% # 当调用运行模式2直接设置导航参数时，应注意在低速周期结束时刻进行
% # 输入输出参数域使用情况表：
%     参数                   初始化引用的域       解算周期引用的域  导航参数设置引用的域  比速度增量及角增量上一周期值设置引用的域
%     in                     g                   所有             外部参数             specVelInc、angleInc
%     cfg、par、initCon、cst 所有                无                无                  无
%     out                   所有                所有              所有                无
%     auxOut                eVC3_n              所有              所有                无
% # 3种解算周期流程及示意图（低速（下标为n）含2个中速（下标为m），中速含2个高速（下标为l））：
%                |l n h|l n h|l n h|l n h|l n h|l n h|l n h|l n h|
%     cycCnt=   0|1    |2    |3    |4    |5    |6    |7    |8    |
%     ------------------------------------------------------------
%     n=        0|1    |     |     |     |2    |     |     |     |
%     ls        i|u    |     |     |     |u    |     |     |     |
%     ------------------------------------------------------------
%     m=        x|0 1  |     |  2  |     |0 1  |     |  2  |     |
%     ns         |i u  |     |  u  |     |i u  |     |  u  |     |
%     ------------------------------------------------------------
%     l=        x|  0 1|    2|  0 1|    2|  0 1|    2|  0 1|    2|
%     hs         |  i u|    u|  i u|    u|  i u|    u|  i u|    u|
%     ------------------------------------------------------------
%     hs         |    c|    c|    c|    c|    c|    c|    c|    c|
%     ------------------------------------------------------------
%     ns         |     |  c  |     |  c  |     |  c  |     |  c  |
%     ------------------------------------------------------------
%     ls         |     |     |     |c    |     |     |     |c    |
%     ------------------------------------------------------------
%     hs         |=====|=====|=====|=====|=====|=====|=====|=====|
%                0 l=1 1 l=2 0 l=1 1 l=2 0 l=1 1 l=2 0 l=1 1 l=2 0
%     ns         |===========|===========|===========|===========|
%                0    m=1    1    m=2    0    m=1    1    m=2    0
%     ls         |=======================|=======================|
%                0          n=1          1          n=2          2
%     其中i为初始化，u为历史量更新，c为解算
%
% Input Arguments
% # mode: int8标量，运行模式：0为初始化，1为正常的导航解算周期，2为导航参数设置，3为比速度增量及角增量上一周期值设置，其它值输出上次运行结果（此时不使用其它输入参数）
% # in: 结构体，输入量，包含以下域：
%     specVelInc: 3*1向量，高速解算周期内的比速度增量，单位m/s
%     angleInc: 3*1向量，高速解算周期内的角增量，单位rad
%     g: 标量，外部给出的重力加速度，单位m/s^2
%     hAltm: 标量，外部传感器（通常是气压计）输出高度，in.specVelInc采样结束时刻值，单位m
%     *Ext: 外部导航参数，如果所有的元素均为有效值，则将替换本函数计算得到的相应导航参数，在正常的导航解算周期模式下，替换时机为相应导航参数的更新时刻，单位均为国际单位制单位
%     beta_m_ext, DvScul_m_ext, DrScrl_m_ext: 3*1向量，外部给出的圆锥、划桨、涡卷效应补偿项，如果不为空或者NaN，则在中速解算周期中使用该值替代内部计算的相应补偿项，默认为空，在中速解算周期中使用
% # cfg: 结构体，配置量，包含以下域：
%     NSInLS: 标量，低速解算周期内的中速解算周期数
%     HSInNS: 标量，中速解算周期内的高速解算周期数
%     useDCMInAttCalc: 逻辑标量，true表示在姿态解算中使用方向余弦矩阵，false表示使用四元数
%     useNormOrthoInAttCalc: 逻辑标量，true表示在姿态解算中使用规范化（及正交化），false表示不使用
%     useHiResPosNSCal: 逻辑标量，true表示在使用高精度中速位置解算算法，false表示不使用
%     useExtGravity: 逻辑标量，true表示在使用外部提供的重力加速度，false表示使用重力模型计算重力
%     navCoordType: 枚举类型，导航坐标系种类，NavCoordType.GEO为地理坐标系，NavCoordType.WAN_AZM为游动方位坐标系，NavCoordType.FREE_AZM为自由方位坐标系
% # par: 结构体，参量，包含以下域：
%     Tl: 标量，高速解算周期，单位s
%     C: 1*3向量，高度通道反馈增益系数，分别反馈至速度二次微分、速度微分、高度微分，均为0时屏蔽高度通道阻尼，高度通道输出与外部传感器高度无关
% # initCon: 结构体，初始条件，包含以下域：
%     L_n: 标量，纬度0时刻量，单位rad
%     L_n1: 标量，纬度-1n时刻量，单位rad
%     lambda_n: 标量，经度0时刻量，单位rad
%     lambda_n1: 标量，经度-1n时刻量，单位rad
%     h_n: 标量，高度0时刻量，单位m
%     h_n1: 标量，高度-1n时刻量，单位m
%     vN_m: 3*1向量，地速0时刻量，单位m/s
%     vN_m1: 3*1向量，地速-1m时刻量，单位m/s
%     vN_n1: 3*1向量，地速-1n时刻量，单位m/s
%     CBL: 3*3矩阵，姿态矩阵0时刻量
%     specVelInc_lx: 3*x向量，0时刻之前的比速度增量，第1列为-1l时刻~0时刻周期，第2列为-2l时刻~-1l时刻周期，依此类推，单位m/s
%     angleInc_lx: 3*x向量，0时刻之前的角度增量，第1列为-1l时刻~0时刻周期，第2列为-2l时刻~-1l时刻周期，依此类推，单位rad
% # cst: 结构体，常量，包含以下域：
%     RE: 标量，地球长半轴，单位m
%     f: 标量，地球扁率，无单位
%     mu: 标量，地球质量与万有引力常数之积，单位m^3/s^2
%     J2, J3: 标量，与地球质量分布相关的常量，无单位
%     omegaIE: 标量，地球自转角速率，单位rad/s
%
% Output Arguments
% # out: 结构体，输出量，包含以下域：
%     cycCnt: 标量，导航解算周期计数
%     LSCnt: 标量，低速导航解算周期计数
%     NSCnt: 标量，低速导航解算周期中的中速导航解算周期计数
%     HSCnt: 标量，中速导航解算周期中的高速导航解算周期计数
%     CNE: 3*3矩阵，位置矩阵
%     L: 标量，纬度，单位rad
%     lambda: 标量，经度，单位rad
%     h: 标量，高度，单位m
%     vN: 3*1向量，导航系下的速度，单位m/s
%     vG: 3*1向量，地理系下的速度，单位m/s
%     CBL: 3*3矩阵，姿态矩阵
%     qBL: 1*4向量，姿态四元数
%     CBN: 3*3矩阵，姿态矩阵
%     CBG: 3*3矩阵，姿态矩阵
%     phi: 标量，滚动角，单位rad
%     theta: 标量，俯仰角，单位rad
%     psi: 标量，偏航角，单位rad
%     psiTrue: 标量，方位角，单位rad
%     alpha: 标量，游动方位角，单位rad
%     gN: 3*1向量，导航系下的重力加速度，单位m/s^2
%     FCN: 3*3矩阵，曲率矩阵
%     omegaIEN: 3*1向量，导航系下的地球自转角速度，单位rad/s
%     omegaENN: 3*1向量，导航系下的位移角速度，单位rad/s
% # auxOut: 结构体，辅助输出量，包含以下域：
%     delh_n: 标量，惯性导航计算的高度与外部传感器输出高度之差，单位m
%     eVC3_n: 标量，高度通道控制信号之一，具体参考REF2第7.3.3节
%     beta_l: 3*1向量，高速解算周期内的圆锥增量
%     DvScul_l: 3*1向量，高速解算周期内的划桨增量
%     DrScrl_l: 3*1向量，高速解算周期内的涡卷增量
%
% Assumptions and Limitations
% # 本算法采用椭圆度计算曲率半径，导航结果和采用偏心率计算曲率半径的算法有差异，当椭圆度越大时差异越明显
%
% References
% # 理论文档“捷联惯性导航数值积分算法” 版本号1.3
% #《应用导航算法工程基础》“捷联惯性导航数值积分算法”

persistent p_cfg_SINI p_par_SINI p_cst_SINI p_mid_SINI p_out_SINI p_auxOut_SINI
persistent p_cfg_SINI_fcnRV2DCM p_cfg_SINI_fcnRV2Quat p_cfg_SINI_fcnDsalpha_l p_cfg_SINI_fcnDsupsilon_l p_cfg_SINI_fcndrScrl_l p_cfg_SINI_fcnDrRot_m  p_cfg_SINI_fcnCalcEarthPar

%% persistent变量初始化
% NOTE: 不应依靠在该cell中对persistent变量赋的初值，因该cell仅在persistent变量不存在时执行
if isempty(p_cfg_SINI)
    p_cfg_SINI.mInn = coder.nullcopy(NaN);
    p_cfg_SINI.lInm = coder.nullcopy(NaN);
    p_cfg_SINI.useDCMInAttCalc = coder.nullcopy(true);
    p_cfg_SINI.useNormOrthoInAttCalc = coder.nullcopy(false);
    p_cfg_SINI.useHiResPosNSCal = coder.nullcopy(true);
    p_cfg_SINI.useExtGravity = coder.nullcopy(false);
    p_cfg_SINI.navCoordType = coder.nullcopy(NavCoordType.WAN_AZM);
end
if isempty(p_par_SINI)
    p_par_SINI.Tl = coder.nullcopy(NaN);
    p_par_SINI.C = coder.nullcopy(NaN(1, 3));
end
if isempty(p_cst_SINI)
    p_cst_SINI.CNL = coder.nullcopy(NaN(3));
    p_cst_SINI.uZNN = coder.nullcopy(NaN(3, 1));
    p_cst_SINI.earth.RE = coder.nullcopy(NaN);
    p_cst_SINI.earth.f = coder.nullcopy(NaN);
    p_cst_SINI.earth.mu = coder.nullcopy(NaN);
    p_cst_SINI.earth.J2 = coder.nullcopy(NaN);
    p_cst_SINI.earth.J3 = coder.nullcopy(NaN);
    p_cst_SINI.earth.omegaIE = coder.nullcopy(NaN);
end
if isempty(p_mid_SINI)
    p_mid_SINI.cycCnt = coder.nullcopy(NaN);
    p_mid_SINI.n = coder.nullcopy(NaN);
    p_mid_SINI.m = coder.nullcopy(NaN);
    p_mid_SINI.l = coder.nullcopy(NaN);
    p_mid_SINI.Dalpha_lx = coder.nullcopy(NaN(3, 2)); % NOTE: 列数需要与选择的函数阶数和初始条件中角增量维数一致（圆锥补偿项阶数、划桨补偿项阶数、角速度双重积分项阶数、涡卷补偿项阶数的最大值）
    p_mid_SINI.Dupsilon_lx = coder.nullcopy(NaN(3, 2)); % NOTE: 列数需要与选择的函数阶数和初始条件中比速度增量维数一致（划桨补偿项阶数、比力双重积分项阶数、涡卷补偿项阶数的最大值）
    p_mid_SINI.DrScrl_l = coder.nullcopy(NaN(3, 1));
    p_mid_SINI.supsilon_l = coder.nullcopy(NaN(3, 1));
    p_mid_SINI.salpha_l = coder.nullcopy(NaN(3, 1));
    p_mid_SINI.CB_mL_n = coder.nullcopy(NaN(3));
    p_mid_SINI.qB_mL_n = coder.nullcopy(NaN(1, 4));
    p_mid_SINI.CB_mL_n1 = coder.nullcopy(NaN(3));
    p_mid_SINI.qB_mL_n1 = coder.nullcopy(NaN(1, 4));
    p_mid_SINI.CB_m1L_n1 = coder.nullcopy(NaN(3));
    p_mid_SINI.qB_m1L_n1 = coder.nullcopy(NaN(1, 4));
    p_mid_SINI.CL_n1L_m = coder.nullcopy(NaN(3));
    p_mid_SINI.CL_n1L_m1 = coder.nullcopy(NaN(3));
    p_mid_SINI.vN_m = coder.nullcopy(NaN(3, 1));
    p_mid_SINI.vN_m1 = coder.nullcopy(NaN(3, 1));
    p_mid_SINI.vN_m2 = coder.nullcopy(NaN(3, 1));
    p_mid_SINI.CNE_n = coder.nullcopy(NaN(3));
    p_mid_SINI.CNE_n1 = coder.nullcopy(NaN(3));
    p_mid_SINI.h_n = coder.nullcopy(NaN);
    p_mid_SINI.h_n1 = coder.nullcopy(NaN);
    p_mid_SINI.sumDrN_m = coder.nullcopy(NaN(3, 1));
    p_mid_SINI.sumDrN_m1 = coder.nullcopy(NaN(3, 1));
    p_mid_SINI.earthPar_n.FCN = coder.nullcopy(NaN(3));
    p_mid_SINI.earthPar_n.rhoNz = coder.nullcopy(NaN);
    p_mid_SINI.earthPar_n.gN = coder.nullcopy(NaN(3, 1));
    p_mid_SINI.earthPar_n.omegaIEN = coder.nullcopy(NaN(3, 1));
    p_mid_SINI.earthPar_n1.FCN = coder.nullcopy(NaN(3));
    p_mid_SINI.earthPar_n1.rhoNz = coder.nullcopy(NaN);
    p_mid_SINI.earthPar_n1.gN = coder.nullcopy(NaN(3, 1));
    p_mid_SINI.earthPar_n1.omegaIEN = coder.nullcopy(NaN(3, 1));
    p_mid_SINI.earthPar_n2.FCN = coder.nullcopy(NaN(3));
    p_mid_SINI.earthPar_n2.rhoNz = coder.nullcopy(NaN);
    p_mid_SINI.earthPar_n2.gN = coder.nullcopy(NaN(3, 1));
    p_mid_SINI.earthPar_n2.omegaIEN = coder.nullcopy(NaN(3, 1));
    p_mid_SINI.eVC3_n = coder.nullcopy(NaN);
    p_mid_SINI.eVC3_n1 = coder.nullcopy(NaN);
end
if isempty(p_out_SINI)
    p_out_SINI.cycCnt = coder.nullcopy(NaN);
    p_out_SINI.LSCnt = coder.nullcopy(NaN);
    p_out_SINI.NSCnt = coder.nullcopy(NaN);
    p_out_SINI.HSCnt = coder.nullcopy(NaN);
    p_out_SINI.CNE = coder.nullcopy(NaN(3));
    p_out_SINI.L = coder.nullcopy(NaN);
    p_out_SINI.lambda = coder.nullcopy(NaN);
    p_out_SINI.h = coder.nullcopy(NaN);
    p_out_SINI.vN = coder.nullcopy(NaN(3, 1));
    p_out_SINI.vG = coder.nullcopy(NaN(3, 1));
    p_out_SINI.CBL = coder.nullcopy(NaN(3));
    p_out_SINI.qBL = coder.nullcopy(NaN(1, 4));
    p_out_SINI.CBN = coder.nullcopy(NaN(3));
    p_out_SINI.CBG = coder.nullcopy(NaN(3));
    p_out_SINI.phi = coder.nullcopy(NaN);
    p_out_SINI.theta = coder.nullcopy(NaN);
    p_out_SINI.psi = coder.nullcopy(NaN);
    p_out_SINI.psiTrue = coder.nullcopy(NaN);
    p_out_SINI.alpha = coder.nullcopy(NaN);
    p_out_SINI.gN = coder.nullcopy(NaN(3, 1));
    p_out_SINI.FCN = coder.nullcopy(NaN(3));
    p_out_SINI.omegaIEN = coder.nullcopy(NaN(3, 1));
    p_out_SINI.omegaENN = coder.nullcopy(NaN(3, 1));
end
if isempty(p_auxOut_SINI)
    p_auxOut_SINI.delh_n = coder.nullcopy(NaN);
    p_auxOut_SINI.eVC3_n = coder.nullcopy(NaN);
    p_auxOut_SINI.beta_l = coder.nullcopy(NaN(3, 1));
    p_auxOut_SINI.DvScul_l = coder.nullcopy(NaN(3, 1));
    p_auxOut_SINI.DrScrl_l = coder.nullcopy(NaN(3, 1));
end
if isempty(p_cfg_SINI_fcnRV2DCM)
    p_cfg_SINI_fcnRV2DCM = @rv2dcm_5thod;
end
if isempty(p_cfg_SINI_fcnRV2Quat)
    p_cfg_SINI_fcnRV2Quat = @rv2quat_5thod;
end
if isempty(p_cfg_SINI_fcnDsalpha_l)
    p_cfg_SINI_fcnDsalpha_l = @Dsalpha_l_2ndod;
end
if isempty(p_cfg_SINI_fcnDsupsilon_l)
    p_cfg_SINI_fcnDsupsilon_l = @Dsupsilon_l_2ndod;
end
if isempty(p_cfg_SINI_fcndrScrl_l)
    p_cfg_SINI_fcndrScrl_l = @drScrl_l_2ndod;
end
if isempty(p_cfg_SINI_fcnDrRot_m)
    p_cfg_SINI_fcnDrRot_m = @DrRot_m_exactform_5thod;
end
if isempty(p_cfg_SINI_fcnCalcEarthPar)
    p_cfg_SINI_fcnCalcEarthPar = @calcearthpar_pgs;
end

switch mode
    case int8(0)
        %% 初始化
        % 常量
        p_cst_SINI.CNL = [0 1 0; 1 0 0; 0 0 -1];
        p_cst_SINI.uZNN = [0 0 1]';
        p_cst_SINI.earth.RE = cst.RE;
        p_cst_SINI.earth.f = cst.f;
        p_cst_SINI.earth.mu = cst.mu;
        p_cst_SINI.earth.J2 = cst.J2;
        p_cst_SINI.earth.J3 = cst.J3;
        p_cst_SINI.earth.omegaIE = cst.omegaIE;
        % 配置
        p_cfg_SINI.mInn = cfg.NSInLS;
        p_cfg_SINI.lInm = cfg.HSInNS;
        p_cfg_SINI.useDCMInAttCalc = cfg.useDCMInAttCalc;
        p_cfg_SINI.useNormOrthoInAttCalc = cfg.useNormOrthoInAttCalc;
        p_cfg_SINI.useHiResPosNSCal = cfg.useHiResPosNSCal;
        p_cfg_SINI.useExtGravity = cfg.useExtGravity;
        p_cfg_SINI.navCoordType = cfg.navCoordType;
        % 参量
        p_par_SINI.Tl = par.Tl; % s
        p_par_SINI.C = par.C; % 高度通道反馈增益系数
        % 零时刻初始化
        p_mid_SINI.cycCnt = 0;
        p_mid_SINI.n = 0;
        p_mid_SINI.Dalpha_lx = horzcat(initCon.angleInc_lx, zeros(3, 1));
        p_mid_SINI.Dupsilon_lx = horzcat(initCon.specVelInc_lx, zeros(3, 1));
        p_mid_SINI.CNE_n = dcmenu2ecef(initCon.L_n, initCon.lambda_n);
        p_mid_SINI.h_n = initCon.h_n;
        p_mid_SINI.vN_m = initCon.vN_m;
        p_mid_SINI.vN_m1 = initCon.vN_m1;
        if cfg.useDCMInAttCalc
            p_mid_SINI.CB_mL_n = initCon.CBL;
            p_mid_SINI.qB_mL_n = NaN(1, 4);
        else
            p_mid_SINI.CB_mL_n = NaN(3);
            p_mid_SINI.qB_mL_n = dcm2quat_cg(initCon.CBL'); % dcm2quat()将CLB转换为qBL
        end
        %地球模型选择
        p_mid_SINI.earthPar_n = p_cfg_SINI_fcnCalcEarthPar(p_mid_SINI.CNE_n, p_mid_SINI.h_n, p_mid_SINI.vN_m(1, 1),  p_cfg_SINI.navCoordType, p_cfg_SINI.useExtGravity, in.g, p_cst_SINI.earth);
        p_mid_SINI.earthPar_n1 = p_cfg_SINI_fcnCalcEarthPar(dcmenu2ecef(initCon.L_n1, initCon.lambda_n1), initCon.h_n1, initCon.vN_n1(1, 1),  p_cfg_SINI.navCoordType, p_cfg_SINI.useExtGravity, in.g, p_cst_SINI.earth);
        p_mid_SINI.eVC3_n = 0;
        % 输出量
        p_out_SINI.cycCnt = p_mid_SINI.cycCnt;
        p_out_SINI.LSCnt = p_mid_SINI.n;
        p_out_SINI.NSCnt = 0;
        p_out_SINI.HSCnt = 0;
        p_out_SINI.CNE = p_mid_SINI.CNE_n;
        p_out_SINI.L = initCon.L_n;
        p_out_SINI.lambda = initCon.lambda_n;
        p_out_SINI.h = p_mid_SINI.h_n;
        p_out_SINI.vN = p_mid_SINI.vN_m;
        p_out_SINI.vG = p_mid_SINI.vN_m;
        if p_cfg_SINI.useDCMInAttCalc
            p_out_SINI.CBL = p_mid_SINI.CB_mL_n;
        else
            p_out_SINI.CBL = quat2dcm_cg(p_mid_SINI.qB_mL_n)'; % quat2dcm()将qBL转换为CLB
        end
        p_out_SINI.qBL = p_mid_SINI.qB_mL_n;
        p_out_SINI.CBN = p_cst_SINI.CNL' * p_out_SINI.CBL;
        p_out_SINI.CBG = p_out_SINI.CBN;
        [p_out_SINI.phi, p_out_SINI.theta, p_out_SINI.psi, p_out_SINI.psiTrue] = dcm2rollpityawhead(p_out_SINI.CBL, 0, 0);
        p_out_SINI.alpha = 0;
        p_out_SINI.gN = p_mid_SINI.earthPar_n.gN;
        p_out_SINI.FCN = p_mid_SINI.earthPar_n.FCN;
        p_out_SINI.omegaIEN = p_mid_SINI.earthPar_n.omegaIEN;
        p_out_SINI.omegaENN = p_mid_SINI.earthPar_n.rhoNz*p_cst_SINI.uZNN + p_out_SINI.FCN*cross(p_cst_SINI.uZNN, p_out_SINI.vN);
        % 辅助输出量
        p_auxOut_SINI.delh_n = NaN;
        p_auxOut_SINI.eVC3_n = p_mid_SINI.eVC3_n;
        p_auxOut_SINI.beta_l = NaN(3, 1);
        p_auxOut_SINI.DvScul_l = NaN(3, 1);
        p_auxOut_SINI.DrScrl_l = NaN(3, 1);
    case int8(1)
        %% 导航解算周期
        mid_delh_n = NaN;
        p_mid_SINI.cycCnt = p_mid_SINI.cycCnt + 1;
        % - - - - - - - - - - - - - - - 低速解算开始- - - - - - - - - - - - - - -
        if mod(p_mid_SINI.cycCnt-1, p_cfg_SINI.lInm*p_cfg_SINI.mInn) == 0
            p_mid_SINI.n = p_mid_SINI.n + 1;
            % 更新低速解算历史量
            p_mid_SINI.CNE_n1 = p_mid_SINI.CNE_n;
            p_mid_SINI.h_n1 = p_mid_SINI.h_n;
            p_mid_SINI.earthPar_n2 = p_mid_SINI.earthPar_n1;
            p_mid_SINI.earthPar_n1 = p_mid_SINI.earthPar_n;
            p_mid_SINI.eVC3_n1 = p_mid_SINI.eVC3_n;
            % 中速解算零时刻初始化
            p_mid_SINI.m = 0;
            if p_cfg_SINI.useDCMInAttCalc
                p_mid_SINI.CB_mL_n1 = p_mid_SINI.CB_mL_n;
            else
                p_mid_SINI.qB_mL_n1 = p_mid_SINI.qB_mL_n;
            end
            p_mid_SINI.CL_n1L_m = eye(3);
            p_mid_SINI.sumDrN_m = zeros(3, 1);
        end
        % ------------------------------中速解算开始------------------------------
        if mod(p_mid_SINI.cycCnt-1, p_cfg_SINI.lInm) == 0
            p_mid_SINI.m = p_mid_SINI.m + 1;
            % 更新中速解算历史量
            if p_cfg_SINI.useDCMInAttCalc
                p_mid_SINI.CB_m1L_n1 = p_mid_SINI.CB_mL_n1;
            else
                p_mid_SINI.qB_m1L_n1 = p_mid_SINI.qB_mL_n1;
                p_mid_SINI.CB_m1L_n1 = quat2dcm_cg(p_mid_SINI.qB_m1L_n1)'; % quat2dcm()将qBL转换为CLB
            end
            p_mid_SINI.CL_n1L_m1 = p_mid_SINI.CL_n1L_m;
            p_mid_SINI.sumDrN_m1 = p_mid_SINI.sumDrN_m;
            p_mid_SINI.vN_m2 = p_mid_SINI.vN_m1;
            p_mid_SINI.vN_m1 = p_mid_SINI.vN_m;
            % 高速解算零时刻初始化
            p_mid_SINI.l = 0;
            bodycoordattupdstep(0);
            bodycoordspecvelupdstep(0);
            p_mid_SINI.DrScrl_l = zeros(3, 1);
            p_mid_SINI.supsilon_l = zeros(3, 1);
            p_mid_SINI.salpha_l = zeros(3, 1);
        end
        % ==============================高速解算开始==============================
        p_mid_SINI.l = p_mid_SINI.l + 1;
        % 更新高速解算历史量，读入器件输出
        p_mid_SINI.Dupsilon_lx = horzcat(in.specVelInc, p_mid_SINI.Dupsilon_lx(:, 1:(end-1)));
        p_mid_SINI.Dalpha_lx = horzcat(in.angleInc, p_mid_SINI.Dalpha_lx(:, 1:(end-1)));
        mid_DrScrl_l1 = p_mid_SINI.DrScrl_l;
        mid_supsilon_l1 = p_mid_SINI.supsilon_l;
        mid_salpha_l1 = p_mid_SINI.salpha_l;
        % 运行高速解算步骤
        [~, ~, mid_alpha_l, mid_alpha_l1, mid_beta_l] = bodycoordattupdstep(1, p_mid_SINI.Dalpha_lx, p_cfg_SINI.useDCMInAttCalc);
        [~, mid_upsilon_l, mid_upsilon_l1, mid_DvScul_l, mid_DvScul_l1] = bodycoordspecvelupdstep(1, p_mid_SINI.Dupsilon_lx, mid_alpha_l1, p_mid_SINI.Dalpha_lx);
        [p_mid_SINI.DrScrl_l, p_mid_SINI.supsilon_l, p_mid_SINI.salpha_l] = pos_hs(mid_DrScrl_l1, mid_supsilon_l1, mid_salpha_l1, mid_DvScul_l1, mid_upsilon_l1, p_mid_SINI.Dupsilon_lx, ...
            mid_alpha_l1, p_mid_SINI.Dalpha_lx, p_par_SINI.Tl, p_cfg_SINI_fcndrScrl_l, p_cfg_SINI_fcnDsupsilon_l, p_cfg_SINI_fcnDsalpha_l);
        % ==============================高速解算结束==============================
        if mod(p_mid_SINI.cycCnt, p_cfg_SINI.lInm) == 0
            % 运行中速解算步骤
            [mid_DvSFB_m1_m] = bodycoordspecvelupdstep(2, p_mid_SINI.Dupsilon_lx, mid_alpha_l1, p_mid_SINI.Dalpha_lx, mid_alpha_l, in.DvScul_m_ext);
            [p_mid_SINI.vN_m, mid_DvSFLI_n1_m, mid_DvGCorN_m, p_mid_SINI.CL_n1L_m] = vel_ns(p_mid_SINI.vN_m1, p_mid_SINI.vN_m2, p_mid_SINI.sumDrN_m1, mid_DvSFB_m1_m, ...
                p_mid_SINI.CB_m1L_n1, p_mid_SINI.CL_n1L_m1, p_mid_SINI.earthPar_n1, p_mid_SINI.earthPar_n2, p_cfg_SINI.mInn, ...
                p_mid_SINI.m, p_par_SINI.Tl*p_cfg_SINI.lInm, p_cst_SINI);
            if ~any(isnan(in.vNExt))
                p_mid_SINI.vN_m = in.vNExt;
            end
            if p_cfg_SINI.useHiResPosNSCal
                if isempty(in.DrScrl_m_ext) || any(isnan(in.DrScrl_m_ext))
                    mid_DrScrl_m = p_mid_SINI.DrScrl_l;
                else
                    mid_DrScrl_m = in.DrScrl_m_ext;
                end
                [~, p_mid_SINI.sumDrN_m] = pos_ns_hires(p_mid_SINI.sumDrN_m1, mid_DrScrl_m, p_mid_SINI.supsilon_l, p_mid_SINI.salpha_l, ...
                    mid_upsilon_l, mid_alpha_l, p_mid_SINI.vN_m1, mid_DvSFLI_n1_m, mid_DvGCorN_m, p_mid_SINI.CL_n1L_m, p_mid_SINI.CL_n1L_m1, ...
                    p_mid_SINI.CB_m1L_n1, p_par_SINI.Tl*p_cfg_SINI.lInm, p_cst_SINI, p_cfg_SINI_fcnDrRot_m);
            else
                [~, p_mid_SINI.sumDrN_m] = pos_ns_trapez(p_mid_SINI.sumDrN_m1, p_mid_SINI.vN_m, p_mid_SINI.vN_m1, p_par_SINI.Tl*p_cfg_SINI.lInm);
            end
            [mid_CB_mB_m1, mid_qB_mB_m1] = bodycoordattupdstep(2, p_mid_SINI.Dalpha_lx, p_cfg_SINI.useDCMInAttCalc, in.beta_m_ext);
            if p_cfg_SINI.useDCMInAttCalc
                p_mid_SINI.CB_mL_n1 = p_mid_SINI.CB_m1L_n1 * mid_CB_mB_m1;
            else
                p_mid_SINI.qB_mL_n1 = quatmultiply(p_mid_SINI.qB_m1L_n1, mid_qB_mB_m1);
            end
        end
        % ------------------------------中速解算结束------------------------------
        if mod(p_mid_SINI.cycCnt, p_cfg_SINI.lInm*p_cfg_SINI.mInn) == 0
            % 运行低速解算步骤
            if any(any(isnan(in.CNEExt))) || isnan(in.hExt)
                [p_mid_SINI.CNE_n, p_mid_SINI.h_n, mid_delh_n] = pos_ls(p_mid_SINI.CNE_n1, p_mid_SINI.h_n1, p_mid_SINI.sumDrN_m, ...
                    p_mid_SINI.earthPar_n1, p_mid_SINI.earthPar_n2, p_par_SINI.Tl*p_cfg_SINI.lInm*p_cfg_SINI.mInn, ...
                    in.hAltm, p_par_SINI.C, p_cst_SINI, p_cfg_SINI_fcnRV2DCM);
                if ~any(any(isnan(in.CNEExt)))
                    p_mid_SINI.CNE_n = in.CNEExt;
                end
                if ~isnan(in.hExt)
                    p_mid_SINI.h_n = in.hExt;
                end
            else
                p_mid_SINI.CNE_n = in.CNEExt;
                p_mid_SINI.h_n = in.hExt;
                mid_delh_n = in.hExt - in.hAltm;
            end
            if any(isnan(in.vNExt)) && any(p_par_SINI.C) % 考虑阻尼的高度通道修正
                [p_mid_SINI.vN_m, p_mid_SINI.eVC3_n] = vel_ls(p_mid_SINI.vN_m, p_par_SINI.Tl*p_cfg_SINI.lInm*p_cfg_SINI.mInn, mid_delh_n, p_mid_SINI.eVC3_n1, p_par_SINI.C, p_cst_SINI);
            end
            p_mid_SINI.earthPar_n = p_cfg_SINI_fcnCalcEarthPar(p_mid_SINI.CNE_n, p_mid_SINI.h_n, p_mid_SINI.vN_m(1, 1),  p_cfg_SINI.navCoordType, p_cfg_SINI.useExtGravity, in.g, p_cst_SINI.earth);
            if p_cfg_SINI.useDCMInAttCalc
                if any(any(isnan(in.CBLExt)))
                    [p_mid_SINI.CB_mL_n] = att_ls_dcm(p_mid_SINI.CB_mL_n1, p_mid_SINI.sumDrN_m, p_mid_SINI.earthPar_n, p_mid_SINI.earthPar_n1, ...
                        p_par_SINI.Tl*p_cfg_SINI.lInm*p_cfg_SINI.mInn, p_cst_SINI, p_cfg_SINI.useNormOrthoInAttCalc, p_cfg_SINI_fcnRV2DCM);
                else
                    p_mid_SINI.CB_mL_n = in.CBLExt;
                end
            else
                if any(isnan(in.qBLExt), 2)
                    [p_mid_SINI.qB_mL_n] = att_ls_quat(p_mid_SINI.qB_mL_n1, p_mid_SINI.sumDrN_m, p_mid_SINI.earthPar_n, p_mid_SINI.earthPar_n1, ...
                        p_par_SINI.Tl*p_cfg_SINI.lInm*p_cfg_SINI.mInn, p_cst_SINI, p_cfg_SINI.useNormOrthoInAttCalc, p_cfg_SINI_fcnRV2Quat);
                else
                    p_mid_SINI.qB_mL_n = in.qBLExt;
                end
            end
        end
        % - - - - - - - - - - - - - - - 低速解算结束- - - - - - - - - - - - - - -
        [p_out_SINI, p_auxOut_SINI, p_mid_SINI] = calcout(p_mid_SINI, p_cfg_SINI.useDCMInAttCalc, mid_delh_n, mid_beta_l, mid_DvScul_l, p_cst_SINI.uZNN);
    case int8(2)
        %% 导航参数设置
        % NOTE: 应将受影响的persistent中间参数全部重新设置
        tmp_needToSetEarthPar = false;
        if ~any(any(isnan(in.CNEExt)))
            p_mid_SINI.CNE_n = in.CNEExt;
            tmp_needToSetEarthPar = true;
        end
        if ~isnan(in.hExt)
            p_mid_SINI.h_n = in.hExt;
            tmp_needToSetEarthPar = true;
        end
        if ~any(isnan(in.vNExt))
            p_mid_SINI.vN_m = in.vNExt;
            tmp_needToSetEarthPar = true;
        end
        if p_cfg_SINI.useDCMInAttCalc
            if ~any(any(isnan(in.CBLExt)))
                p_mid_SINI.CB_mL_n = in.CBLExt;
            end
        else
            if ~any(isnan(in.qBLExt), 2)
                p_mid_SINI.qB_mL_n = in.qBLExt;
            end
        end
        if tmp_needToSetEarthPar
            p_mid_SINI.earthPar_n = p_cfg_SINI_fcnCalcEarthPar(p_mid_SINI.CNE_n, p_mid_SINI.h_n, p_mid_SINI.vN_m(1, 1),  p_cfg_SINI.navCoordType, p_cfg_SINI.useExtGravity, in.g, p_cst_SINI.earth);
        end
        [p_out_SINI, p_auxOut_SINI, p_mid_SINI] = calcout(p_mid_SINI, p_cfg_SINI.useDCMInAttCalc, NaN, NaN(3, 1), NaN(3, 1), p_cst_SINI.uZNN);
    case int8(3)
        %% 比速度增量及角增量上一周期值设置
        if ~any(isnan(in.specVelInc))
            p_mid_SINI.Dupsilon_lx(:, 1) = in.specVelInc;
        end
        if ~any(isnan(in.angleInc))
            p_mid_SINI.Dalpha_lx(:, 1) = in.angleInc;
        end
    otherwise
        %% 仅输出，不进行任何操作
end
out = p_out_SINI;
auxOut = p_auxOut_SINI;
end

function [out, auxOut, mid] = calcout(mid, useDCMInAttCalc, mid_delh_n, mid_beta_l, mid_DvScul_l, uZNN) %#codegen
% NOTE: mid为按引用传递，不要修改其值
out.cycCnt = mid.cycCnt;
out.LSCnt = mid.n;
out.NSCnt = mid.m;
out.HSCnt = mid.l;
if useDCMInAttCalc
    tmp_CBL = mid.CB_mL_n; % CBI_mLI_n在低速解算时更新
else
    tmp_CBL = quat2dcm_cg(mid.qB_mL_n)'; % quat2dcm()将qBL转换为CLB
end
[tmp_L, tmp_lambda, tmp_vG, tmp_CBN, tmp_CBG, tmp_phi, tmp_theta, tmp_psi, tmp_psiTrue, tmp_alpha] = wholenavparam(mid.CNE_n, mid.vN_m', tmp_CBL);
out.CNE = mid.CNE_n; % CNE_n在低速解算时更新
out.L = tmp_L;
out.lambda = tmp_lambda;
out.h = mid.h_n; % h_n在低速解算时更新
out.vN = mid.vN_m; % vN_m在中速解算时更新
out.vG = tmp_vG';
out.CBL = tmp_CBL;
out.qBL = mid.qB_mL_n;
out.CBN = tmp_CBN;
out.CBG = tmp_CBG;
out.phi = tmp_phi;
out.theta = tmp_theta;
out.psi = tmp_psi;
out.psiTrue = tmp_psiTrue;
out.alpha = tmp_alpha;
out.gN = mid.earthPar_n.gN;
out.FCN = mid.earthPar_n.FCN;
out.omegaIEN = mid.earthPar_n.omegaIEN;
out.omegaENN = mid.earthPar_n.rhoNz*uZNN + mid.earthPar_n.FCN*cross(uZNN, mid.vN_m);

auxOut.delh_n = mid_delh_n;
auxOut.eVC3_n = mid.eVC3_n;
auxOut.beta_l = mid_beta_l;
auxOut.DvScul_l = mid_DvScul_l;
auxOut.DrScrl_l = mid.DrScrl_l;
end